#!/usr/bin/env python3
"""
Enhanced LLM Bridge Voice Control Node - 增强版LLM桥接语音控制节点

集成了原始 voice_control_node.py 的高级功能和 llm_bridge_voice_control_node.py 的LLM集成优势

主要特性：
- 完整的ROS2参数系统
- 高级安全机制和紧急停止
- 导航系统集成
- 音频反馈系统
- 性能监控
- MessagePack 和 JSON 双格式解析
- 智能处理分割消息和重组
- 持久连接和自动重连机制
- 优化的命令执行时序
- 增强的错误处理和恢复
"""

import asyncio
import json
import time
import threading
from enum import Enum
from typing import Optional, Dict, Any, Callable
import re

import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy

import websockets
from websockets.exceptions import ConnectionClosed, InvalidURI, WebSocketException

from geometry_msgs.msg import Twist
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan

# 尝试导入 msgpack，如果没有则使用 JSON 回退
try:
    import msgpack
    MSGPACK_AVAILABLE = True
except ImportError:
    MSGPACK_AVAILABLE = False

# Import EchoKit message handler
try:
    from .echokit_message_handler import EchoKitMessageHandler
    ECHOKIT_HANDLER_AVAILABLE = True
except ImportError:
    ECHOKIT_HANDLER_AVAILABLE = False


class VoiceControlState(Enum):
    """States for the voice control system."""
    DISCONNECTED = "disconnected"
    CONNECTED = "connected"
    LISTENING = "listening"
    PROCESSING = "processing"
    EXECUTING = "executing"
    TIMEOUT = "timeout"
    EMERGENCY = "emergency"


class ControlMode(Enum):
    """Control modes for navigation integration."""
    VOICE_CONTROL = "voice_control"
    NAVIGATION = "navigation"
    VOICE_OVERRIDE = "voice_override"
    MIXED = "mixed"


class WebSocketClient:
    """
    Enhanced WebSocket client for connecting to EchoKit server.
    
    Handles connection management, message parsing, and automatic reconnection
    with exponential backoff.
    """
    
    def __init__(self, url: str, reconnect_interval: float = 5.0, 
                 connection_timeout: float = 10.0, logger=None):
        """
        Initialize WebSocket client.
        
        Args:
            url: WebSocket server URL
            reconnect_interval: Time between reconnection attempts (seconds)
            connection_timeout: Connection timeout (seconds)
            logger: ROS2 logger instance
        """
        self.url = url
        self.reconnect_interval = reconnect_interval
        self.connection_timeout = connection_timeout
        self.logger = logger
        
        self.websocket = None
        self.connected = False
        self.running = False
        self.reconnect_task = None
        
        # 指数退避重连机制
        self.reconnect_attempts = 0
        self.max_reconnect_attempts = 10
        self.base_reconnect_interval = reconnect_interval
        
        # Event loop and thread management
        self.loop = None
        self.thread = None
        
        # Message handlers
        self.message_handlers: Dict[str, Callable] = {}
        self.connection_handlers: Dict[str, Callable] = {}
        
        # Initialize EchoKit message handler if available
        if ECHOKIT_HANDLER_AVAILABLE:
            self.echokit_handler = EchoKitMessageHandler(logger)
        else:
            self.echokit_handler = None
        
    def add_message_handler(self, message_type: str, handler: Callable):
        """Add a handler for specific message types."""
        self.message_handlers[message_type] = handler
        
    def add_connection_handler(self, event: str, handler: Callable):
        """Add handlers for connection events (connected, disconnected, error)."""
        self.connection_handlers[event] = handler
    
    def start(self):
        """Start the WebSocket client in a separate thread."""
        if self.running:
            return
            
        self.running = True
        self.thread = threading.Thread(target=self._run_event_loop, daemon=True)
        self.thread.start()
        
        if self.logger:
            self.logger.info(f'WebSocket client started, connecting to {self.url}')
    
    def stop(self):
        """Stop the WebSocket client and cleanup resources."""
        self.running = False
        
        if self.loop and self.loop.is_running():
            # Schedule cleanup in the event loop
            asyncio.run_coroutine_threadsafe(self._cleanup(), self.loop)
        
        if self.thread and self.thread.is_alive():
            self.thread.join(timeout=5.0)
            
        if self.logger:
            self.logger.info('WebSocket client stopped')
    
    def _run_event_loop(self):
        """Run the asyncio event loop in the thread."""
        self.loop = asyncio.new_event_loop()
        asyncio.set_event_loop(self.loop)
        
        try:
            self.loop.run_until_complete(self._connect_with_retry())
        except Exception as e:
            if self.logger:
                self.logger.error(f'WebSocket event loop error: {e}')
        finally:
            self.loop.close()
    
    async def _connect_with_retry(self):
        """Connect to WebSocket server with automatic retry and exponential backoff."""
        while self.running:
            try:
                await self._connect()
                # 连接成功，重置重连计数
                self.reconnect_attempts = 0
                # If we get here, connection was successful and closed normally
                if self.running:
                    if self.logger:
                        self.logger.info(f'Connection closed, retrying in {self.base_reconnect_interval}s')
                    await asyncio.sleep(self.base_reconnect_interval)
                    
            except (ConnectionClosed, WebSocketException, OSError) as e:
                if self.running:
                    # 计算指数退避延迟
                    delay = min(self.base_reconnect_interval * (2 ** self.reconnect_attempts), 60.0)
                    self.reconnect_attempts = min(self.reconnect_attempts + 1, self.max_reconnect_attempts)
                    
                    if self.logger:
                        self.logger.warn(f'WebSocket connection error: {e}, retrying in {delay:.1f}s (attempt {self.reconnect_attempts})')
                    self._call_connection_handler('error', str(e))
                    await asyncio.sleep(delay)
                    
            except Exception as e:
                if self.logger:
                    self.logger.error(f'Unexpected WebSocket error: {e}')
                if self.running:
                    # 对于未预期的错误也使用指数退避
                    delay = min(self.base_reconnect_interval * (2 ** self.reconnect_attempts), 60.0)
                    self.reconnect_attempts = min(self.reconnect_attempts + 1, self.max_reconnect_attempts)
                    await asyncio.sleep(delay)
    
    async def _connect(self):
        """Establish WebSocket connection and handle messages."""
        websocket = None
        try:
            if self.logger:
                self.logger.info(f'Attempting to connect to {self.url}')
                
            # Use asyncio.wait_for for timeout instead of websockets timeout parameter
            websocket = await asyncio.wait_for(
                websockets.connect(
                    self.url,
                    ping_interval=20,  # 20秒ping间隔
                    ping_timeout=10,   # 10秒ping超时
                    close_timeout=10,  # 10秒关闭超时
                    max_size=2**16,    # 减小消息大小
                    compression=None   # 禁用压缩以减少延迟
                ),
                timeout=self.connection_timeout
            )
            
            self.websocket = websocket
            self.connected = True
            
            if self.logger:
                self.logger.info('WebSocket connected successfully')
            
            # Send robot client identification message
            await self._send_robot_identification(websocket)
            
            self._call_connection_handler('connected')
            
            # Message handling loop
            try:
                async for message in websocket:
                    if not self.running:
                        break
                        
                    try:
                        await self._handle_message(message)
                    except Exception as e:
                        if self.logger:
                            self.logger.error(f'Error handling message: {e}')
            except ConnectionClosed:
                if self.logger:
                    self.logger.info('WebSocket connection closed normally')
            except Exception as e:
                if self.logger:
                    self.logger.error(f'Error in message loop: {e}')
                raise
                            
        except InvalidURI as e:
            if self.logger:
                self.logger.error(f'Invalid WebSocket URI: {self.url} - {e}')
            raise
            
        except asyncio.TimeoutError:
            if self.logger:
                self.logger.warn(f'WebSocket connection timeout after {self.connection_timeout}s')
            raise
            
        finally:
            self.connected = False
            if websocket and not websocket.closed:
                try:
                    await websocket.close()
                except Exception:
                    pass
            self.websocket = None
            if self.running:
                self._call_connection_handler('disconnected')
    
    async def _handle_message(self, message):
        """Parse and handle incoming WebSocket messages."""
        try:
            # Handle binary messages (MessagePack format from EchoKit server)
            if isinstance(message, bytes):
                if self.logger:
                    self.logger.debug(f'Received binary message of {len(message)} bytes')
                
                # Try to parse as MessagePack using EchoKit message handler
                if self.echokit_handler:
                    try:
                        parsed_message = self.echokit_handler.parse_message(message)
                        if parsed_message:
                            await self._process_parsed_message(parsed_message, 'messagepack')
                            return
                    except Exception as parse_error:
                        if self.logger:
                            self.logger.debug(f'EchoKit handler parsing error: {parse_error}')
                
                # Fallback: try MessagePack directly
                if MSGPACK_AVAILABLE:
                    try:
                        data = msgpack.unpackb(message, raw=False)
                        await self._process_parsed_message(data, 'messagepack_direct')
                        return
                    except Exception as msgpack_error:
                        if self.logger:
                            self.logger.debug(f'Direct MessagePack parsing error: {msgpack_error}')
                
                # Fallback: try to find JSON in binary data
                try:
                    json_start = message.find(b'{')
                    if json_start != -1:
                        json_part = message[json_start:].decode('utf-8')
                        data = json.loads(json_part)
                        await self._process_parsed_message(data, 'json_from_binary')
                        return
                except Exception:
                    pass
                
                if self.logger:
                    self.logger.debug('Could not parse binary message')
                return
            
            # Handle text messages
            if isinstance(message, str):
                if self.logger:
                    self.logger.info(f'Received text message: {message[:200]}{"..." if len(message) > 200 else ""}')
                
                # Use EchoKit message handler to parse the message
                if self.echokit_handler:
                    try:
                        parsed_message = self.echokit_handler.parse_message(message)
                        if parsed_message:
                            await self._process_parsed_message(parsed_message, 'echokit_handler')
                            return
                    except Exception as parse_error:
                        if self.logger:
                            self.logger.debug(f'EchoKit handler text parsing error: {parse_error}')
                
                # Fallback to direct JSON parsing
                try:
                    data = json.loads(message)
                    await self._process_parsed_message(data, 'json_direct')
                    return
                except json.JSONDecodeError:
                    if self.logger:
                        self.logger.debug(f'Could not parse text message as JSON: {message}')
                
                # Handle as plain text
                await self._handle_plain_text(message)
                        
        except Exception as e:
            if self.logger:
                self.logger.error(f'Error in message handler: {e}')
    
    async def _process_parsed_message(self, data: Dict[str, Any], source: str):
        """Process parsed message data."""
        try:
            message_type = data.get('type', 'unknown')
            
            if self.logger:
                self.logger.info(f'Processing message type: {message_type} (source: {source})')
            
            # Handle different message types
            if message_type in self.message_handlers:
                handler = self.message_handlers[message_type]
                try:
                    if asyncio.iscoroutinefunction(handler):
                        await handler(data)
                    else:
                        handler(data)
                except Exception as handler_error:
                    if self.logger:
                        self.logger.error(f'Handler error for {message_type}: {handler_error}')
            else:
                if self.logger:
                    self.logger.debug(f'No handler for message type: {message_type}')
                    
        except Exception as e:
            if self.logger:
                self.logger.error(f'Error processing parsed message: {e}')
    
    async def _handle_plain_text(self, message: str):
        """Handle plain text messages."""
        # Check if it's a structured command (JSON format)
        if message.strip().startswith('{') and message.strip().endswith('}'):
            try:
                data = json.loads(message.strip())
                await self._process_parsed_message(data, 'plain_text_json')
                return
            except json.JSONDecodeError:
                pass
        
        # Handle as generic text message
        if 'text' in self.message_handlers:
            handler = self.message_handlers['text']
            try:
                if asyncio.iscoroutinefunction(handler):
                    await handler({'type': 'text', 'content': message})
                else:
                    handler({'type': 'text', 'content': message})
            except Exception as e:
                if self.logger:
                    self.logger.error(f'Text handler error: {e}')
    
    def _call_connection_handler(self, event: str, data=None):
        """Call connection event handlers in a thread-safe way."""
        if event in self.connection_handlers:
            handler = self.connection_handlers[event]
            try:
                handler(data)
            except Exception as e:
                if self.logger:
                    self.logger.error(f'Error in connection handler for {event}: {e}')
    
    async def send_message(self, message: Dict):
        """Send a message to the WebSocket server."""
        if not self.connected or not self.websocket:
            if self.logger:
                self.logger.warn('Cannot send message: WebSocket not connected')
            return False
            
        try:
            message_str = json.dumps(message)
            await self.websocket.send(message_str)
            
            if self.logger:
                self.logger.debug(f'Sent message: {message}')
            return True
            
        except Exception as e:
            if self.logger:
                self.logger.error(f'Failed to send message: {e}')
            return False
    
    def send_message_sync(self, message: Dict):
        """Send a message synchronously (thread-safe)."""
        if self.loop and self.loop.is_running():
            future = asyncio.run_coroutine_threadsafe(self.send_message(message), self.loop)
            try:
                return future.result(timeout=60.0)
            except asyncio.TimeoutError:
                if self.logger:
                    self.logger.error('Timeout sending message')
                return False
        return False
    
    async def _send_robot_identification(self, websocket):
        """Send robot client identification to EchoKit server."""
        try:
            # Send robot client identification message
            identification_message = {
                "type": "robot_client_connected",
                "client_type": "robot",
                "node_name": "llm_bridge_voice_control_node",
                "capabilities": ["voice_command", "movement_control", "llm_integration"],
                "timestamp": int(time.time() * 1000)
            }
            
            await websocket.send(json.dumps(identification_message))
            
            if self.logger:
                self.logger.info(f'Sent robot identification: {identification_message}')
                
            # Also try alternative identification formats that EchoKit might expect
            alt_messages = [
                {"type": "register", "client_type": "robot", "robot_id": "echokitbot_llm_bridge"},
                {"type": "robot_ready", "status": "ready"},
                {"type": "subscribe", "events": ["voice_command", "robot_command", "llm_response"]}
            ]
            
            for alt_msg in alt_messages:
                await websocket.send(json.dumps(alt_msg))
                if self.logger:
                    self.logger.debug(f'Sent alternative identification: {alt_msg}')
                await asyncio.sleep(0.1)
                
        except Exception as e:
            if self.logger:
                self.logger.error(f'Failed to send robot identification: {e}')
    
    async def _cleanup(self):
        """Cleanup WebSocket resources."""
        if self.websocket:
            try:
                await self.websocket.close()
            except Exception:
                pass
            self.websocket = None
        
        self.connected = False
    
    def is_connected(self) -> bool:
        """Check if WebSocket is currently connected."""
        return self.connected


class LLMBridgeVoiceControlNode(Node):
    """
    Enhanced LLM Bridge Voice Control Node for EchokitBot robot.
    
    This node combines the LLM integration capabilities with advanced features
    from the original voice_control_node, providing a comprehensive solution
    for voice-controlled robot navigation.
    """
    
    def __init__(self):
        """Initialize the enhanced LLM bridge voice control node."""
        super().__init__('llm_bridge_voice_control_node')
        
        # Initialize state
        self.current_state = VoiceControlState.DISCONNECTED
        self.emergency_stop_active = False
        self.last_command_time = time.time()
        self.current_velocity = Twist()
        
        # Enhanced safety state tracking
        self.safety_timeout_active = False
        self.emergency_stop_timestamp = None
        self.velocity_ramp_start_time = None
        self.target_velocity = Twist()
        self.velocity_ramp_active = False
        
        # Navigation integration state
        self.control_mode = ControlMode.VOICE_CONTROL
        self.navigation_active = False
        self.voice_override_active = False
        self.voice_override_start_time = None
        self.last_navigation_command = Twist()
        self.last_navigation_time = 0.0
        
        # Command execution state
        self.command_count = 0
        self.executing_command = False
        
        # JSON 缓冲区用于处理分割的消息
        self.json_buffer = ""
        self.last_message_time = 0
        
        # WebSocket client (will be initialized later)
        self.websocket_client = None
        
        # Declare and load parameters
        self._declare_parameters()
        self._load_parameters()
        
        # Create QoS profiles
        self._setup_qos_profiles()
        
        # Create publishers
        self._setup_publishers()
        
        # Create timers
        self._setup_timers()
        
        # Initialize WebSocket client
        self._setup_websocket_client()
        
        # Log successful initialization
        self.get_logger().info('🚀 Enhanced LLM Bridge Voice Control Node initialized successfully')
        self.get_logger().info(f'📡 Server URL: {self.server_url}')
        self.get_logger().info(f'⚡ Max linear velocity: {self.max_linear_velocity} m/s')
        self.get_logger().info(f'🔄 Max angular velocity: {self.max_angular_velocity} rad/s')
        self.get_logger().info('✅ 支持 MessagePack 和 JSON 双格式解析')
        self.get_logger().info('🛡️ 集成高级安全机制和参数系统')
        self.get_logger().info('🎯 LLM集成 + 企业级功能')
        
        # Publish initial status
        self._publish_status("initialized")
    
    def _declare_parameters(self):
        """Declare all ROS2 parameters with default values and constraints."""
        try:
            # Server connection parameters
            self.declare_parameter('server.url', 'ws://192.168.2.28:8060/ws/robot')
            self.declare_parameter('server.reconnect_interval', 5.0)
            self.declare_parameter('server.connection_timeout', 30.0)
            
            # Movement parameters with safety constraints
            self.declare_parameter('movement.max_linear_velocity', 0.5)
            self.declare_parameter('movement.max_angular_velocity', 1.0)
            self.declare_parameter('movement.default_linear_velocity', 0.3)
            self.declare_parameter('movement.default_angular_velocity', 0.5)
            
            # Safety parameters with validation ranges
            self.declare_parameter('safety.command_timeout', 5.0)
            self.declare_parameter('safety.emergency_stop_timeout', 10.0)
            self.declare_parameter('safety.velocity_ramp_time', 0.5)
            self.declare_parameter('safety.max_acceleration', 1.0)
            self.declare_parameter('safety.collision_distance', 0.3)
            
            # Audio parameters
            self.declare_parameter('audio.confirmation_enabled', True)
            self.declare_parameter('audio.language', 'auto')
            self.declare_parameter('audio.volume', 0.8)
            self.declare_parameter('audio.feedback_timeout', 2.0)
            
            # Command recognition parameters
            self.declare_parameter('commands.confidence_threshold', 0.7)
            self.declare_parameter('commands.language_detection', True)
            self.declare_parameter('commands.case_sensitive', False)
            self.declare_parameter('commands.retry_attempts', 3)
            
            # Navigation integration parameters
            self.declare_parameter('navigation.voice_override_duration', 5.0)
            self.declare_parameter('navigation.priority_mode', 'voice_priority')
            self.declare_parameter('navigation.smooth_transition', True)
            self.declare_parameter('navigation.nav_timeout', 2.0)
            self.declare_parameter('navigation.monitor_nav_commands', True)
            
            # Debug and logging parameters
            self.declare_parameter('debug.enable_verbose_logging', False)
            self.declare_parameter('debug.log_websocket_messages', False)
            self.declare_parameter('debug.performance_monitoring', False)
            
            self.get_logger().info('All parameters declared successfully')
            
        except Exception as e:
            self.get_logger().error(f'Failed to declare parameters: {e}')
            raise
    
    def _load_parameters(self):
        """Load parameter values from ROS2 parameter server."""
        try:
            # Server parameters
            self.server_url = self.get_parameter('server.url').get_parameter_value().string_value
            self.reconnect_interval = self.get_parameter('server.reconnect_interval').get_parameter_value().double_value
            self.connection_timeout = self.get_parameter('server.connection_timeout').get_parameter_value().double_value
            
            # Movement parameters
            self.max_linear_velocity = self.get_parameter('movement.max_linear_velocity').get_parameter_value().double_value
            self.max_angular_velocity = self.get_parameter('movement.max_angular_velocity').get_parameter_value().double_value
            self.default_linear_velocity = self.get_parameter('movement.default_linear_velocity').get_parameter_value().double_value
            self.default_angular_velocity = self.get_parameter('movement.default_angular_velocity').get_parameter_value().double_value
            
            # Safety parameters
            self.command_timeout = self.get_parameter('safety.command_timeout').get_parameter_value().double_value
            self.emergency_stop_timeout = self.get_parameter('safety.emergency_stop_timeout').get_parameter_value().double_value
            self.velocity_ramp_time = self.get_parameter('safety.velocity_ramp_time').get_parameter_value().double_value
            self.max_acceleration = self.get_parameter('safety.max_acceleration').get_parameter_value().double_value
            self.collision_distance = self.get_parameter('safety.collision_distance').get_parameter_value().double_value
            
            # Audio parameters
            self.confirmation_enabled = self.get_parameter('audio.confirmation_enabled').get_parameter_value().bool_value
            self.language = self.get_parameter('audio.language').get_parameter_value().string_value
            self.volume = self.get_parameter('audio.volume').get_parameter_value().double_value
            self.feedback_timeout = self.get_parameter('audio.feedback_timeout').get_parameter_value().double_value
            
            # Command parameters
            self.confidence_threshold = self.get_parameter('commands.confidence_threshold').get_parameter_value().double_value
            self.language_detection = self.get_parameter('commands.language_detection').get_parameter_value().bool_value
            self.case_sensitive = self.get_parameter('commands.case_sensitive').get_parameter_value().bool_value
            self.retry_attempts = self.get_parameter('commands.retry_attempts').get_parameter_value().integer_value
            
            # Navigation parameters
            self.voice_override_duration = self.get_parameter('navigation.voice_override_duration').get_parameter_value().double_value
            self.priority_mode = self.get_parameter('navigation.priority_mode').get_parameter_value().string_value
            self.smooth_transition = self.get_parameter('navigation.smooth_transition').get_parameter_value().bool_value
            self.nav_timeout = self.get_parameter('navigation.nav_timeout').get_parameter_value().double_value
            self.monitor_nav_commands = self.get_parameter('navigation.monitor_nav_commands').get_parameter_value().bool_value
            
            # Debug parameters
            self.enable_verbose_logging = self.get_parameter('debug.enable_verbose_logging').get_parameter_value().bool_value
            self.log_websocket_messages = self.get_parameter('debug.log_websocket_messages').get_parameter_value().bool_value
            self.performance_monitoring = self.get_parameter('debug.performance_monitoring').get_parameter_value().bool_value
            
            self.get_logger().info('Parameters loaded successfully')
            
        except Exception as e:
            self.get_logger().error(f'Failed to load parameters: {e}')
            self.get_logger().warn('Using default parameter values')
            self._set_default_parameters()
    
    def _set_default_parameters(self):
        """Set default parameter values if loading fails."""
        # Server parameters
        self.server_url = 'ws://192.168.2.28:8060/ws/robot'
        self.reconnect_interval = 5.0
        self.connection_timeout = 30.0
        
        # Movement parameters
        self.max_linear_velocity = 0.5
        self.max_angular_velocity = 1.0
        self.default_linear_velocity = 0.3
        self.default_angular_velocity = 0.5
        
        # Safety parameters
        self.command_timeout = 5.0
        self.emergency_stop_timeout = 10.0
        self.velocity_ramp_time = 0.5
        self.max_acceleration = 1.0
        self.collision_distance = 0.3
        
        # Audio parameters
        self.confirmation_enabled = True
        self.language = 'auto'
        self.volume = 0.8
        self.feedback_timeout = 2.0
        
        # Command parameters
        self.confidence_threshold = 0.7
        self.language_detection = True
        self.case_sensitive = False
        self.retry_attempts = 3
        
        # Navigation parameters
        self.voice_override_duration = 5.0
        self.priority_mode = 'voice_priority'
        self.smooth_transition = True
        self.nav_timeout = 2.0
        self.monitor_nav_commands = True
        
        # Debug parameters
        self.enable_verbose_logging = False
        self.log_websocket_messages = False
        self.performance_monitoring = False
    
    def _setup_qos_profiles(self):
        """Setup QoS profiles for different types of communication."""
        # Best effort QoS for cmd_vel (allows packet loss for real-time control)
        self.cmd_vel_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            depth=1
        )
        
        # Reliable QoS for status messages
        self.status_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE,
            depth=10
        )
        
        # Sensor QoS for odometry and laser scan
        self.sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            depth=5
        )
    
    def _setup_publishers(self):
        """Create ROS2 publishers for robot control and status."""
        try:
            # Main command velocity publisher
            self.cmd_vel_publisher = self.create_publisher(
                Twist, 
                '/cmd_vel', 
                self.cmd_vel_qos
            )
            
            # Legacy publisher names for backward compatibility
            self.cmd_vel_pub = self.cmd_vel_publisher
            
            # Voice text publisher
            self.voice_text_publisher = self.create_publisher(
                String, 
                '/voice_text', 
                self.status_qos
            )
            
            # Structured command publisher
            self.structured_command_publisher = self.create_publisher(
                String, 
                '/structured_voice_command', 
                self.status_qos
            )
            
            # Status publisher for system monitoring
            self.status_publisher = self.create_publisher(
                String, 
                '/voice_control_status', 
                self.status_qos
            )
            
            # Legacy status publisher
            self.status_pub = self.create_publisher(
                String, 
                '/voice_control/status', 
                self.status_qos
            )
            
            # Voice feedback publisher
            self.voice_feedback_pub = self.create_publisher(
                String, 
                '/voice_control/feedback', 
                self.status_qos
            )
            
            # Control mode publisher
            self.control_mode_pub = self.create_publisher(
                String, 
                '/voice_control/mode', 
                self.status_qos
            )
            
            self.get_logger().info('Publishers created successfully')
            
        except Exception as e:
            self.get_logger().error(f'Failed to create publishers: {e}')
            raise
    
    def _setup_timers(self):
        """Create ROS2 timers for periodic tasks."""
        try:
            # Safety timeout timer
            self.safety_timer = self.create_timer(
                0.1,  # 10Hz for safety checks
                self._safety_check_callback
            )
            
            # Status publishing timer
            self.status_timer = self.create_timer(
                1.0,  # 1Hz for status updates
                self._status_callback
            )
            
            # Performance monitoring timer (if enabled)
            if self.performance_monitoring:
                self.performance_timer = self.create_timer(
                    5.0,  # Every 5 seconds
                    self._performance_callback
                )
            
            self.get_logger().info('Timers created successfully')
            
        except Exception as e:
            self.get_logger().error(f'Failed to create timers: {e}')
            raise
    
    def _setup_websocket_client(self):
        """Initialize and configure WebSocket client for EchoKit server."""
        try:
            # Create WebSocket client
            self.websocket_client = WebSocketClient(
                url=self.server_url,
                reconnect_interval=self.reconnect_interval,
                connection_timeout=self.connection_timeout,
                logger=self.get_logger()
            )
            
            # Register message handlers
            self.websocket_client.add_message_handler('voice_command', self._handle_voice_command)
            self.websocket_client.add_message_handler('robot_command', self._handle_robot_command)
            self.websocket_client.add_message_handler('action', self._handle_action_command)
            self.websocket_client.add_message_handler('emergency_stop', self._handle_emergency_stop)
            self.websocket_client.add_message_handler('status', self._handle_server_status)
            self.websocket_client.add_message_handler('llm_response', self._handle_llm_response)
            self.websocket_client.add_message_handler('timed_motion', self._handle_timed_motion)
            self.websocket_client.add_message_handler('continuous_motion', self._handle_continuous_motion)
            self.websocket_client.add_message_handler('stop', self._handle_stop_command)
            self.websocket_client.add_message_handler('text', self._handle_text_message)
            
            # Register connection handlers
            self.websocket_client.add_connection_handler('connected', self._on_websocket_connected)
            self.websocket_client.add_connection_handler('disconnected', self._on_websocket_disconnected)
            self.websocket_client.add_connection_handler('error', self._on_websocket_error)
            
            # Start WebSocket client
            self.websocket_client.start()
            
            self.get_logger().info('WebSocket client initialized and started')
            
        except Exception as e:
            self.get_logger().error(f'Failed to setup WebSocket client: {e}')
            self.websocket_client = None   
 
    # Timer callbacks
    def _safety_check_callback(self):
        """Periodic safety check to ensure robot safety."""
        try:
            current_time = time.time()
            
            # Check for command timeout
            if (current_time - self.last_command_time) > self.command_timeout:
                if not self.safety_timeout_active:
                    self._handle_safety_timeout()
                    self.safety_timeout_active = True
            else:
                self.safety_timeout_active = False
            
            # Check emergency stop timeout
            if self.emergency_stop_active and self.emergency_stop_timestamp:
                if (current_time - self.emergency_stop_timestamp) > self.emergency_stop_timeout:
                    self.get_logger().info('Emergency stop timeout reached, clearing emergency state')
                    self.emergency_stop_active = False
                    self.emergency_stop_timestamp = None
                    self.current_state = VoiceControlState.CONNECTED if self.websocket_client and self.websocket_client.is_connected() else VoiceControlState.DISCONNECTED
            
        except Exception as e:
            self.get_logger().error(f'Error in safety check: {e}')
    
    def _status_callback(self):
        """Periodic status publishing."""
        try:
            # Publish current state
            status_msg = f"state:{self.current_state.value}"
            if self.websocket_client:
                status_msg += f",connected:{self.websocket_client.is_connected()}"
            status_msg += f",emergency:{self.emergency_stop_active}"
            status_msg += f",mode:{self.control_mode.value}"
            
            self._publish_status(status_msg)
            
        except Exception as e:
            self.get_logger().error(f'Error in status callback: {e}')
    
    def _performance_callback(self):
        """Performance monitoring callback."""
        if self.performance_monitoring:
            try:
                # Log performance metrics
                self.get_logger().info(f'Performance: State={self.current_state.value}, '
                                     f'Connected={self.websocket_client.is_connected() if self.websocket_client else False}, '
                                     f'Emergency={self.emergency_stop_active}')
            except Exception as e:
                self.get_logger().error(f'Error in performance callback: {e}')
    
    def _handle_safety_timeout(self):
        """Handle command timeout by stopping the robot and updating state."""
        self.current_state = VoiceControlState.TIMEOUT
        
        # Stop the robot
        stop_twist = Twist()
        self.cmd_vel_publisher.publish(stop_twist)
        self.current_velocity = stop_twist
        
        self.get_logger().warn('Safety timeout activated - no commands received within timeout period')
    
    # Enhanced message parsing methods
    def parse_server_message(self, message):
        """解析服务器消息，支持 MessagePack 和 JSON 格式"""
        try:
            # 尝试 MessagePack 解析（服务器主要使用这种格式）
            if isinstance(message, (bytes, bytearray)):
                try:
                    if MSGPACK_AVAILABLE:
                        data = msgpack.unpackb(message, raw=False)
                        return self.handle_msgpack_event(data)
                except:
                    # 如果不是 MessagePack，尝试作为 JSON 处理
                    message = message.decode('utf-8') if isinstance(message, bytes) else message
            
            # JSON 处理逻辑
            return self.parse_json_message(message)
            
        except Exception as e:
            self.get_logger().debug(f"消息解析失败: {e}")
            return None
    
    def handle_msgpack_event(self, data):
        """处理 MessagePack 格式的服务器事件"""
        if not isinstance(data, dict):
            return None
            
        event_type = data.get('type') or list(data.keys())[0] if data else None
        
        if event_type == 'ASR':
            # ASR 结果，检查是否包含机器人命令
            text = data.get('text', '')
            self.get_logger().info(f"收到 ASR: {text}")
            return self.extract_robot_command_from_text(text)
            
        elif event_type == 'Action':
            # 直接的动作命令
            action = data.get('action', '')
            self.get_logger().info(f"收到 Action: {action}")
            return self.parse_action_command(action)
            
        elif event_type == 'StartAudio':
            # TTS 开始，检查文本中是否包含命令
            text = data.get('text', '')
            self.get_logger().info(f"收到 StartAudio: {text}")
            return self.extract_robot_command_from_text(text)
            
        return None
    
    def extract_robot_command_from_text(self, text):
        """从文本中提取机器人命令 JSON"""
        if not text:
            return None
            
        # 查找 JSON 对象
        json_patterns = [
            r'\{[^{}]*"type"[^{}]*\}',  # 简单 JSON
            r'\{(?:[^{}]|{[^{}]*})*\}',  # 嵌套 JSON
        ]
        
        for pattern in json_patterns:
            matches = re.findall(pattern, text)
            for match in matches:
                try:
                    data = json.loads(match)
                    if self.is_valid_robot_command(data):
                        return data
                except:
                    continue
        
        return None
    
    def parse_action_command(self, action):
        """解析动作命令字符串为机器人命令"""
        try:
            # 如果 action 本身就是 JSON 字符串
            if action.startswith('{') and action.endswith('}'):
                data = json.loads(action)
                if self.is_valid_robot_command(data):
                    return data
            
            # 如果是简单的动作名称，转换为标准命令
            action_map = {
                'forward': {'type': 'continuous_motion', 'linear': 0.3, 'angular': 0.0},
                'backward': {'type': 'continuous_motion', 'linear': -0.3, 'angular': 0.0},
                'left': {'type': 'continuous_motion', 'linear': 0.0, 'angular': 1.0},
                'right': {'type': 'continuous_motion', 'linear': 0.0, 'angular': -1.0},
                'stop': {'type': 'stop'},
            }
            
            if action.lower() in action_map:
                cmd = action_map[action.lower()].copy()
                cmd['original_command'] = action
                cmd['description'] = f"Execute {action}"
                return cmd
                
        except Exception as e:
            self.get_logger().debug(f"动作解析失败: {e}")
        
        return None
    
    def parse_json_message(self, message):
        """解析 JSON 消息，处理分割情况"""
        current_time = time.time()
        
        # 如果距离上次消息时间超过 1 秒，清空缓冲区
        if current_time - self.last_message_time > 1.0:
            self.json_buffer = ""
        
        self.last_message_time = current_time
        
        # 添加到缓冲区
        self.json_buffer += message
        
        # 尝试解析完整的 JSON
        return self.extract_complete_json()
    
    def extract_complete_json(self):
        """从缓冲区提取完整的 JSON 对象"""
        buffer = self.json_buffer.strip()
        
        # 查找完整的 JSON 对象
        brace_count = 0
        start_pos = -1
        
        for i, char in enumerate(buffer):
            if char == '{':
                if start_pos == -1:
                    start_pos = i
                brace_count += 1
            elif char == '}':
                brace_count -= 1
                if brace_count == 0 and start_pos != -1:
                    # 找到完整的 JSON
                    json_str = buffer[start_pos:i+1]
                    try:
                        data = json.loads(json_str)
                        if self.is_valid_robot_command(data):
                            # 清空已处理的部分
                            self.json_buffer = buffer[i+1:]
                            return data
                    except:
                        continue
        
        return None
    
    def is_valid_robot_command(self, data):
        """验证是否为有效的机器人命令"""
        if not isinstance(data, dict):
            return False
            
        cmd_type = data.get('type')
        if cmd_type not in ['timed_motion', 'continuous_motion', 'stop']:
            return False
            
        if cmd_type in ['timed_motion', 'continuous_motion']:
            if 'linear' not in data or 'angular' not in data:
                return False
            try:
                float(data['linear'])
                float(data['angular'])
            except:
                return False
                
        if cmd_type == 'timed_motion' and 'duration' not in data:
            return False
            
        return True 
   
    # WebSocket message handlers
    def _handle_voice_command(self, message_data: Dict[str, Any]):
        """Handle voice command messages from EchoKit server."""
        try:
            self.get_logger().info(f'Received voice command data: {message_data}')
            
            # Extract command information with validation
            command = message_data.get('command', '').lower().strip()
            action = message_data.get('action')
            
            if not command:
                self.get_logger().warn(f'Missing command in message: {message_data}')
                return
                
            if not action:
                self.get_logger().warn(f'Missing action in message: {message_data}')
                return
            
            self.get_logger().info(f'Processing voice command: {command}, action: {action}')
            
            # 重要修复：检查并清除紧急状态（如果是停止命令）
            if command == 'stop' and self.emergency_stop_active:
                self.get_logger().info('Stop command received in voice handler, clearing emergency state')
                self.emergency_stop_active = False
                self.emergency_stop_timestamp = None
            
            # Update state safely
            try:
                self.current_state = VoiceControlState.PROCESSING
                self.last_command_time = time.time()
            except Exception as state_error:
                self.get_logger().error(f'State update error: {state_error}')
            
            # Create velocity command with validation
            try:
                twist = Twist()
                
                # Safely extract values
                if isinstance(action, dict):
                    linear_val = action.get('linear', 0.0)
                    angular_val = action.get('angular', 0.0)
                else:
                    self.get_logger().warn(f'Action is not a dict: {action}')
                    linear_val = 0.0
                    angular_val = 0.0
                
                twist.linear.x = float(linear_val)
                twist.angular.z = float(angular_val)
                
                # Apply safety limits with parameter validation
                max_linear = getattr(self, 'max_linear_velocity', 0.5)
                max_angular = getattr(self, 'max_angular_velocity', 1.0)
                
                twist.linear.x = max(-max_linear, min(max_linear, twist.linear.x))
                twist.angular.z = max(-max_angular, min(max_angular, twist.angular.z))
                
                self.get_logger().info(f'Created twist: linear={twist.linear.x:.2f}, angular={twist.angular.z:.2f}')
                
            except Exception as twist_error:
                self.get_logger().error(f'Twist creation error: {twist_error}')
                return
            
            # Publish command safely
            try:
                if hasattr(self, 'cmd_vel_publisher') and self.cmd_vel_publisher:
                    self.cmd_vel_publisher.publish(twist)
                    self.current_velocity = twist
                    self.get_logger().info(f'Command published successfully')
                else:
                    self.get_logger().error(f'cmd_vel_publisher not available')
                    return
                    
            except Exception as pub_error:
                self.get_logger().error(f'Publishing error: {pub_error}')
                return
            
            # Update state and publish status
            try:
                self.current_state = VoiceControlState.EXECUTING
                self._publish_status(f'executing_command:{command}')
                
                # Publish voice text and structured command
                self.publish_voice_text(command)
                self.publish_structured_command(message_data)
                
                self.get_logger().info(f'Voice command executed successfully: {command} -> linear: {twist.linear.x:.2f}, angular: {twist.angular.z:.2f}')
                
            except Exception as status_error:
                self.get_logger().error(f'Status update error: {status_error}')
            
        except Exception as e:
            self.get_logger().error(f'Critical voice command error: {e}')
    
    def _handle_robot_command(self, data: Dict):
        """Handle robot command messages from EchoKit server."""
        try:
            command_type = data.get('command', '').lower()
            
            if command_type in ['move', 'navigate', 'go']:
                # Extract movement parameters
                linear_x = data.get('linear_x', 0.0)
                angular_z = data.get('angular_z', 0.0)
                
                # Create and publish twist message
                twist = Twist()
                twist.linear.x = max(-self.max_linear_velocity, min(self.max_linear_velocity, linear_x))
                twist.angular.z = max(-self.max_angular_velocity, min(self.max_angular_velocity, angular_z))
                
                self.cmd_vel_publisher.publish(twist)
                self.current_velocity = twist
                self.last_command_time = time.time()
                
                self.get_logger().info(f'Robot command executed: {command_type} -> linear: {twist.linear.x:.2f}, angular: {twist.angular.z:.2f}')
                
            elif command_type == 'stop':
                self._handle_stop_command(data)
                
        except Exception as e:
            self.get_logger().error(f'Error handling robot command: {e}')
    
    def _handle_action_command(self, data: Dict):
        """Handle action command messages from EchoKit server."""
        try:
            action = data.get('action', '').lower()
            
            if action in ['forward', 'backward', 'left', 'right', 'stop']:
                # 重要修复：如果是停止命令且当前处于紧急状态，先清除紧急状态
                if action == 'stop' and self.emergency_stop_active:
                    self.get_logger().info('Stop command received, clearing emergency state')
                    self.emergency_stop_active = False
                    self.emergency_stop_timestamp = None
                    self.current_state = VoiceControlState.CONNECTED if self.websocket_client and self.websocket_client.is_connected() else VoiceControlState.DISCONNECTED
                
                # Map actions to velocities
                action_map = {
                    'forward': {'linear': self.default_linear_velocity, 'angular': 0.0},
                    'backward': {'linear': -self.default_linear_velocity, 'angular': 0.0},
                    'left': {'linear': 0.0, 'angular': self.default_angular_velocity},
                    'right': {'linear': 0.0, 'angular': -self.default_angular_velocity},
                    'stop': {'linear': 0.0, 'angular': 0.0}
                }
                
                if action in action_map:
                    twist = Twist()
                    twist.linear.x = action_map[action]['linear']
                    twist.angular.z = action_map[action]['angular']
                    
                    self.cmd_vel_publisher.publish(twist)
                    self.current_velocity = twist
                    self.last_command_time = time.time()
                    
                    # 更新状态为正常执行状态
                    if action == 'stop':
                        self.current_state = VoiceControlState.CONNECTED if self.websocket_client and self.websocket_client.is_connected() else VoiceControlState.DISCONNECTED
                        self.get_logger().info(f'Stop command executed successfully - robot stopped')
                    else:
                        self.current_state = VoiceControlState.EXECUTING
                        self.get_logger().info(f'Action command executed: {action}')
                    
        except Exception as e:
            self.get_logger().error(f'Error handling action command: {e}')
    
    def _handle_timed_motion(self, data: Dict):
        """Handle timed motion commands."""
        try:
            # 防止重复执行
            if self.executing_command:
                self.get_logger().warn("命令执行中，跳过新命令")
                return
                
            self.executing_command = True
            self.command_count += 1
            
            try:
                original_cmd = data.get('original_command', f'Timed Motion #{self.command_count}')
                self.get_logger().info(f"🎯 执行定时运动命令 #{self.command_count}: {original_cmd}")
                
                # 发布语音文本和结构化命令
                self.publish_voice_text(original_cmd)
                self.publish_structured_command(data)
                self._publish_status(f"executing_timed_motion")
                
                # 执行定时运动
                asyncio.create_task(self.execute_timed_motion(data))
                
            finally:
                pass  # executing_command will be cleared in execute_timed_motion
                
        except Exception as e:
            self.get_logger().error(f'Error handling timed motion: {e}')
            self.executing_command = False
    
    def _handle_continuous_motion(self, data: Dict):
        """Handle continuous motion commands."""
        try:
            # 防止重复执行
            if self.executing_command:
                self.get_logger().warn("命令执行中，跳过新命令")
                return
                
            self.executing_command = True
            self.command_count += 1
            
            try:
                original_cmd = data.get('original_command', f'Continuous Motion #{self.command_count}')
                self.get_logger().info(f"🎯 执行连续运动命令 #{self.command_count}: {original_cmd}")
                
                # 发布语音文本和结构化命令
                self.publish_voice_text(original_cmd)
                self.publish_structured_command(data)
                self._publish_status(f"executing_continuous_motion")
                
                # 执行连续运动
                asyncio.create_task(self.execute_continuous_motion(data))
                
            finally:
                pass  # executing_command will be cleared in execute_continuous_motion
                
        except Exception as e:
            self.get_logger().error(f'Error handling continuous motion: {e}')
            self.executing_command = False
    
    def _handle_stop_command(self, data: Dict):
        """Handle stop commands."""
        try:
            self.get_logger().info("🛑 执行停止命令")
            
            # Clear emergency state if active
            if self.emergency_stop_active:
                self.get_logger().info('Stop command clearing emergency state')
                self.emergency_stop_active = False
                self.emergency_stop_timestamp = None
            
            # Stop the robot
            stop_twist = Twist()
            self.cmd_vel_publisher.publish(stop_twist)
            self.current_velocity = stop_twist
            self.last_command_time = time.time()
            
            # Update to normal connected state
            self.current_state = VoiceControlState.CONNECTED if self.websocket_client and self.websocket_client.is_connected() else VoiceControlState.DISCONNECTED
            
            # Publish status
            self.publish_voice_text("停止")
            self.publish_structured_command(data)
            self._publish_status("stopped")
            
            self.executing_command = False
            
            self.get_logger().info('Stop command executed - robot stopped')
            
        except Exception as e:
            self.get_logger().error(f'Error handling stop command: {e}')
    
    def _handle_emergency_stop(self, data: Dict):
        """Handle emergency stop messages from EchoKit server."""
        try:
            self.emergency_stop_active = True
            self.emergency_stop_timestamp = time.time()
            self.current_state = VoiceControlState.EMERGENCY
            
            # Stop the robot immediately
            stop_twist = Twist()
            self.cmd_vel_publisher.publish(stop_twist)
            self.current_velocity = stop_twist
            
            self.executing_command = False
            
            self.get_logger().warn('Emergency stop activated')
            self._publish_status("emergency_stop")
            
        except Exception as e:
            self.get_logger().error(f'Error handling emergency stop: {e}')
    
    def _handle_llm_response(self, data: Dict):
        """Handle LLM response messages that contain robot commands."""
        try:
            # Extract command from LLM response
            content = data.get('content', '')
            if '[' in content and ']' in content:
                # Parse bracket commands like [forward], [stop], etc.
                import re
                matches = re.findall(r'\[([^\]]+)\]', content)
                if matches:
                    command = matches[0].lower().strip()
                    self._handle_action_command({'action': command})
                    
        except Exception as e:
            self.get_logger().error(f'Error handling LLM response: {e}')
    
    def _handle_server_status(self, data: Dict):
        """Handle status messages from EchoKit server."""
        try:
            status = data.get('status', 'unknown')
            self.get_logger().info(f'EchoKit server status: {status}')
            
        except Exception as e:
            self.get_logger().error(f'Error handling server status: {e}')
    
    def _handle_text_message(self, data: Dict):
        """Handle plain text messages."""
        try:
            content = data.get('content', '')
            self.get_logger().info(f'Received text message: {content}')
            self.publish_voice_text(content)
            
        except Exception as e:
            self.get_logger().error(f'Error handling text message: {e}')
    
    # WebSocket connection handlers
    def _on_websocket_connected(self, data=None):
        """Handle WebSocket connection established."""
        self.get_logger().info('Connected to EchoKit server as robot client')
        self.current_state = VoiceControlState.CONNECTED
        self._publish_status('connected_to_echokit_as_robot')
    
    def _on_websocket_disconnected(self, data=None):
        """Handle WebSocket connection lost."""
        self.get_logger().warn('Disconnected from EchoKit server')
        self.current_state = VoiceControlState.DISCONNECTED
        self._publish_status('disconnected_from_echokit')
        # Emergency stop when connection is lost
        self._handle_emergency_stop({'type': 'connection_lost'})
    
    def _on_websocket_error(self, error_msg):
        """Handle WebSocket connection errors."""
        self.get_logger().error(f'WebSocket error: {error_msg}')
        self.current_state = VoiceControlState.DISCONNECTED
        self._publish_status(f'websocket_error:{error_msg}') 
   
    # Command execution methods
    async def execute_timed_motion(self, data):
        """执行定时运动 - 基于测试节点的成功经验"""
        try:
            twist = Twist()
            twist.linear.x = float(data.get('linear', 0.0))
            twist.angular.z = float(data.get('angular', 0.0))
            duration = float(data.get('duration', 1.0))
            
            # Apply safety limits
            twist.linear.x = max(-self.max_linear_velocity, min(self.max_linear_velocity, twist.linear.x))
            twist.angular.z = max(-self.max_angular_velocity, min(self.max_angular_velocity, twist.angular.z))
            
            self.get_logger().info(f"⏱️  定时运动: linear={twist.linear.x}, angular={twist.angular.z}, duration={duration}s")
            
            # 在单独线程中执行定时运动
            def timed_motion():
                start_time = time.time()
                while time.time() - start_time < duration and self.websocket_client and self.websocket_client.is_connected():
                    self.cmd_vel_publisher.publish(twist)
                    time.sleep(0.1)
                
                # 停止
                stop_twist = Twist()
                self.cmd_vel_publisher.publish(stop_twist)
                self.current_velocity = stop_twist
                self.executing_command = False
                self.get_logger().info(f"✅ 定时运动完成 #{self.command_count}")
                self._publish_status("ready")
            
            threading.Thread(target=timed_motion, daemon=True).start()
            
        except Exception as e:
            self.get_logger().error(f'Error in execute_timed_motion: {e}')
            self.executing_command = False
    
    async def execute_continuous_motion(self, data):
        """执行连续运动 - 基于测试节点的成功经验"""
        try:
            twist = Twist()
            twist.linear.x = float(data.get('linear', 0.0))
            twist.angular.z = float(data.get('angular', 0.0))
            
            # Apply safety limits
            twist.linear.x = max(-self.max_linear_velocity, min(self.max_linear_velocity, twist.linear.x))
            twist.angular.z = max(-self.max_angular_velocity, min(self.max_angular_velocity, twist.angular.z))
            
            self.get_logger().info(f"🔄 连续运动: linear={twist.linear.x}, angular={twist.angular.z}")
            
            # 发送几次确保命令被接收
            for _ in range(3):
                self.cmd_vel_publisher.publish(twist)
                await asyncio.sleep(0.05)
            
            self.current_velocity = twist
            self.last_command_time = time.time()
            self.current_state = VoiceControlState.EXECUTING
            self.executing_command = False
            
        except Exception as e:
            self.get_logger().error(f'Error in execute_continuous_motion: {e}')
            self.executing_command = False
    
    async def execute_stop(self):
        """执行停止 - 基于测试节点的成功经验"""
        try:
            self.get_logger().info("🛑 停止运动")
            
            stop_twist = Twist()
            for _ in range(3):
                self.cmd_vel_publisher.publish(stop_twist)
                await asyncio.sleep(0.05)
            
            self.current_velocity = stop_twist
            self.last_command_time = time.time()
            self.executing_command = False
            
        except Exception as e:
            self.get_logger().error(f'Error in execute_stop: {e}')
            self.executing_command = False
    
    # Enhanced command processing methods
    async def execute_command(self, data):
        """执行命令 - 统一的命令执行入口"""
        if not isinstance(data, dict):
            return
            
        cmd_type = data.get('type')
        if cmd_type not in ['timed_motion', 'continuous_motion', 'stop']:
            return
        
        # 防止重复执行
        if self.executing_command:
            self.get_logger().warn("命令执行中，跳过新命令")
            return
            
        self.executing_command = True
        self.command_count += 1
        
        try:
            original_cmd = data.get('original_command', f'Command #{self.command_count}')
            self.get_logger().info(f"🎯 执行命令 #{self.command_count}: {original_cmd}")
            
            # 发布语音文本
            self.publish_voice_text(original_cmd)
            
            # 发布结构化命令
            self.publish_structured_command(data)
            
            # 发布状态
            self._publish_status(f"executing_{cmd_type}")
            
            # 执行运动命令
            if cmd_type == 'timed_motion':
                await self.execute_timed_motion(data)
            elif cmd_type == 'continuous_motion':
                await self.execute_continuous_motion(data)
            elif cmd_type == 'stop':
                await self.execute_stop()
                
        except Exception as e:
            self.get_logger().error(f'Error executing command: {e}')
        finally:
            if cmd_type != 'timed_motion':  # timed_motion handles this in its thread
                self.executing_command = False
                self._publish_status("ready")
    
    # Publishing methods
    def publish_voice_text(self, text: str):
        """发布语音文本"""
        try:
            msg = String()
            msg.data = text
            self.voice_text_publisher.publish(msg)
            self.get_logger().info(f"Published voice text: {text}")
        except Exception as e:
            self.get_logger().error(f'Error publishing voice text: {e}')
    
    def publish_structured_command(self, command_data: Dict[str, Any]):
        """发布结构化命令"""
        try:
            msg = String()
            msg.data = json.dumps(command_data)
            self.structured_command_publisher.publish(msg)
            self.get_logger().info(f"Published structured command: {command_data}")
        except Exception as e:
            self.get_logger().error(f'Error publishing structured command: {e}')
    
    def publish_status(self, status: str):
        """发布状态信息 - 兼容方法"""
        self._publish_status(status)
    
    def _publish_status(self, status: str):
        """Publish status message to the status topics."""
        try:
            status_msg = String()
            status_msg.data = f"{int(time.time() * 1000)}:{status}"
            
            # Publish to both status topics for compatibility
            self.status_publisher.publish(status_msg)
            self.status_pub.publish(status_msg)
            
        except Exception as e:
            self.get_logger().error(f'Error publishing status: {e}')
    
    # Utility methods
    def cleanup_websocket(self):
        """Cleanup WebSocket client resources."""
        if self.websocket_client:
            self.websocket_client.stop()
            self.websocket_client = None
    
    def destroy_node(self):
        """清理资源"""
        try:
            self.get_logger().info('Shutting down Enhanced LLM Bridge Voice Control Node...')
            
            # Stop the robot
            stop_twist = Twist()
            if hasattr(self, 'cmd_vel_publisher'):
                self.cmd_vel_publisher.publish(stop_twist)
            
            # Cleanup WebSocket
            self.cleanup_websocket()
            
            # Call parent cleanup
            super().destroy_node()
            
            self.get_logger().info('Node shutdown complete')
            
        except Exception as e:
            self.get_logger().error(f'Error during node shutdown: {e}')


def main(args=None):
    """Main entry point for the enhanced LLM bridge voice control node."""
    print("🚀 Enhanced LLM Bridge Voice Control Node")
    print("=" * 50)
    print("集成了原始 voice_control_node.py 的高级功能")
    print("和 llm_bridge_voice_control_node.py 的LLM集成优势")
    print("=" * 50)
    
    rclpy.init(args=args)
    
    node = None
    try:
        node = LLMBridgeVoiceControlNode()
        
        print("✅ 节点启动成功")
        print("🎤 等待语音命令...")
        print("📡 监控话题:")
        print("   - /cmd_vel (运动控制)")
        print("   - /voice_text (语音文本)")
        print("   - /structured_voice_command (结构化命令)")
        print("   - /voice_control_status (状态信息)")
        print("   - /voice_control/status (兼容状态)")
        print("   - /voice_control/feedback (语音反馈)")
        print("   - /voice_control/mode (控制模式)")
        print("")
        print("🛡️ 安全特性:")
        print("   - 命令超时检测")
        print("   - 紧急停止机制")
        print("   - 速度限制和渐变")
        print("   - 连接状态监控")
        print("")
        print("🎯 LLM集成特性:")
        print("   - MessagePack 和 JSON 双格式解析")
        print("   - 智能消息重组")
        print("   - 结构化命令处理")
        print("")
        print("按 Ctrl+C 停止")
        
        # Log startup completion
        node.get_logger().info('Enhanced LLM Bridge Voice Control Node startup completed')
        node.get_logger().info('Ready to receive voice commands via EchoKit WebSocket connection')
        
        # Spin the node
        rclpy.spin(node)
        
    except KeyboardInterrupt:
        print("\n🛑 停止节点")
        if node:
            node.get_logger().info('Keyboard interrupt received, shutting down...')
    except Exception as e:
        print(f"\n❌ 节点错误: {e}")
        if node:
            node.get_logger().error(f'Node error: {e}')
    finally:
        if node:
            node.destroy_node()
        rclpy.shutdown()
        print("👋 节点已关闭")


if __name__ == '__main__':
    main()